// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KukaLBRControlAgentTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Hardware
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Drawing;
    using System.IO;
    using System.Reflection;
    using System.Runtime.Serialization;
    using System.Threading;

    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Hardware.Runtime;
    using Microsoft.Robotics.Hardware.Runtime.Robotiq;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Manipulation.Runtime.Arm;
    using Microsoft.Robotics.Manipulation.Runtime.MotionControl;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using ShoNS.Array;
    using Test.Robotics.Hardware.KukaLBR.KukaLBR4Tests;
    using Test.Robotics.Numerics;
    using Test.Robotics.RdsSim;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Test the manipulator agent pipeline
    /// </summary>
    [TestClass]
    public class ManipulatorAgentTest
    {
        /// <summary>
        /// Delegate for getting Pose from the current arm status
        /// </summary>
        /// <param name="status">Current arm status</param>
        /// <returns>Pose of the End Effector</returns>
        private delegate Pose GetCurrentPoseFromStatus(ManipulatorStatus status);

        /// <summary>
        /// Check a known problematic target position
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationKukaArmCartesianMoveAgentPrecisionRegression()
        {
            Pose target = new Pose(
                new Vector3(
                    -0.22435146999748024,
                    -0.84797844376786546,
                    -0.8520867376830833),
                new Quaternion(
                    0.54081071908718059,
                    0.674457826187321,
                    0.16268637641493464,
                    0.47556655658704444));

            this.ManipulationKukaArmCartesianMoveAgentTest(target);
        }

        /// <summary>
        /// Build a manipulation agent pipeline with the manipulation agents and a test FRI.
        /// Send a cartesian move message down the pipeline and wait till
        /// status messages show the cartesian move is complete
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationKukaArmCartesianRandomMoveAgentTest()
        {
            this.ManipulationKukaArmCartesianMoveAgentTest(PoseTest.GenerateRandomPose(-1.0, 1.0));
        }

        /// <summary>
        /// Build a manipulation agent pipeline with the manipulation agents and a test sending motion commands to KRL interpreter over FRI.
        /// Send a cartesian move message down the pipeline and wait till
        /// status messages show the cartesian move is complete
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationKukaArmKrlCartesianRandomMoveAgentTest()
        {
            this.ManipulationKukaArmCartesianMoveAgentTest(PoseTest.GenerateRandomPose(-1.0, 1.0), true, false);
        }

        /// <summary>
        /// Build a manipulation agent pipeline with the manipulation agents and a test FRI.
        /// Send a cartesian move message down the pipeline and wait till
        /// status messages show the cartesian move is complete
        /// The message sent is known to have reliably reproduced a bug caused by an unstable 
        /// conversion from a rotation matrix to a quaternion.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationKukaArmCartesianFrameToQuaternionRegression()
        {
            this.ManipulationKukaArmCartesianMoveAgentTest(new Pose(new Vector3(0.609048, 0.04151, 0.5342), new Quaternion(0.04224, 0.8873, 0.42032, 0.18487)));
            this.ManipulationKukaArmCartesianMoveAgentTest(new Pose(new Vector3(-0.372, -0.467, 0.315), new Quaternion(0.666, -0.746, 0.000, 0.00)));
            this.ManipulationKukaArmCartesianMoveAgentTest(new Pose(new Vector3(-0.500, -0.200, 0.417), new Quaternion(0.835, -0.550, 0.000, 0.00)));
            this.ManipulationKukaArmCartesianMoveAgentTest(new Pose(new Vector3(-0.436, -0.471, 0.455), new Quaternion(0.693, -0.721, 0.000, 0.00)));
        }

        /// <summary>
        /// Build a manipulation agent pipeline with the manipulation agents and a test FRI.
        /// Send a spline start message down the pipeline and wait till
        /// status messages show the move is complete
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationKukaArmSplineAgentTest()
        {
            double[] target1 = new double[] { 0, 0, 0, 0, 0, 0, 0 };
            double time1 = 1;
            double[] target2 = new double[] { 1, 2, 3, 3, 2, 1, 2 };
            double time2 = 3;

            CubicSpline cs = new CubicSpline(target1.Length, 2);

            for (int i = 0; i < target1.Length; ++i)
            {
                // only need to set 2nd position as test uses current position as start
                cs.TargetPositions[i, 1] = target1[i];
            }

            cs.Times[1] = time1;

            this.ManipulationKukaSplineAgentTest(cs, 0.1, 0.001, 0.015);

            for (int i = 0; i < target1.Length; ++i)
            {
                // only need to set 2nd position as test uses current position as start
                cs.TargetPositions[i, 1] = target2[i];
            }

            cs.Times[1] = time2;

            this.ManipulationKukaSplineAgentTest(cs, 0.1, 0.001);
        }

        /// <summary>
        /// Test that an exception occurs when a negative time is given for first spline time.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ApplicationException))]
        [Ignore]
        public void ManipulationKukaArmSplineAgentInvalidTimeTest()
        {
            double[] target1 = new double[] { 10, 10, 0, 0, 0, 0, 10 };

            CubicSpline cs = new CubicSpline(target1.Length, 2);
            cs.Times[0] = -1;
            cs.Times[1] = 10;
            for (int i = 0; i < target1.Length; ++i)
            {
                cs.TargetPositions[i, 1] = target1[i];
            }

            this.ManipulationKukaSplineAgentTest(cs, 0.1, 0.001);
        }

        /// <summary>
        /// Test that an exception occurs when spline times are not monotonically increasing
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ApplicationException))]
        [Ignore]
        public void ManipulationKukaArmSplineAgentNonIncreasingTimeTest()
        {
            double[] target1 = new double[] { 0, 0, 0, 0, 0, 0, 0 };

            CubicSpline cs = new CubicSpline(target1.Length, 3);
            cs.Times[1] = 2;
            cs.Times[2] = 1;

            this.ManipulationKukaSplineAgentTest(cs, 0.1, 0.001);
        }

        /// <summary>
        /// Used to test Cartesian moves of the LBR
        /// </summary>
        /// <param name="target"> Target of move. </param>
        /// <param name="useKrl"> Indicates whether or not to use KRL motion interpreter. </param>
        /// <param name="linearMotion"> Indicates whether this motion is linear motion. </param>
        public void ManipulationKukaArmCartesianMoveAgentTest(Pose target, bool useKrl = false, bool linearMotion = true)
        {
            FriSimulator testFri = new FriSimulator();
            ArmControlKuka armControl = new ArmControlKuka(testFri);

            // Create a pipeline using a simulated FRI
            AgentLocator producerLocator = new AgentLocator();

            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 10, 1);

            KukaLBRControlAgent manipAgent = new KukaLBRControlAgent(
                        "manipulationAgent",
                        timer,
                        armControl,
                        0.75,
                        1,
                        0.5,
                        useKrl,
                        1);

            IAgentHost timerHost = producerLocator.Register(timer);
            IAgentHost manipHost = producerLocator.Register(manipAgent);
            IForwarder<LinearCartesianMotionControlMessage> armLinearMotionControlInjector = manipHost.GetControlPort<LinearCartesianMotionControlMessage>();
            IForwarder<CartesianGoalPositionControlMessage> armCartesianGoalMotionControlInjector = manipHost.GetControlPort<CartesianGoalPositionControlMessage>();

            MessageCollectorAgent<ManipulatorStatusMessage> collector = new MessageCollectorAgent<ManipulatorStatusMessage>("collector", manipAgent);
            IAgentHost collectorHost = producerLocator.Register(collector);

            long originatingTime = 10;

            try
            {
                manipHost.Activate();

                // Wait for FRI control to start
                while (testFri.GetRequestCount <= 1)
                {
                    Thread.Sleep(testFri.CycleTimeinMsec);
                }

                if (true == linearMotion)
                {
                    double posTolerance = 0.005;
                    LinearCartesianMotionControlMessage armMoveMessage = new LinearCartesianMotionControlMessage(originatingTime, target, posTolerance);
                    armLinearMotionControlInjector.Post(armMoveMessage);
                }
                else
                {
                    double translationalTolerance = 0.005;
                    double rotationalTolerance = 0.005;
                    CartesianGoalPositionControlMessage armMoveMessage = new CartesianGoalPositionControlMessage(originatingTime, target, translationalTolerance, rotationalTolerance);
                    armCartesianGoalMotionControlInjector.Post(armMoveMessage);
                }

                collectorHost.Activate();
                collector.Wait(500);

                long startTicks = Stopwatch.GetTimestamp();
                long elapsedTicks = 0;

                // TimeOut in 1 Sec
                long timeOutTicks = Stopwatch.Frequency * 1;
                this.MonitorCartesianMoveProcess(collector, target, this.ExtractPoseFromStatus, timeOutTicks, out elapsedTicks);

                Assert.IsTrue(elapsedTicks <= timeOutTicks, "ManipulationAgent test timed out");
            }
            finally
            {
                collectorHost.Deactivate();
                manipHost.Deactivate();
            }
        }

        /// <summary>
        /// Used to test LBR Cartesian touch
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("E2E")]
        [DeploymentItem("Microsoft.Robotics.Manipulation.ControllerStateMachine.dll")]
        [DeploymentItem("FRI_Remote.lib")]
        [DeploymentItem("Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4Controller.dll")]
        public void ManipulationKukaArmCartesianTouchAgentTest()
        {
            IAgentHost manipHost = null;
            IAgentHost collectorHost = null;
            long timeOutTicks = 0;
            long elapsedTicks = 0;
            bool unexpectedContact = false;

            try
            {
                FriSimulator testFri = new FriSimulator();
                testFri.TcpForce = new Vector3(1, 1, 1);

                ArmControlKuka armControl = new ArmControlKuka(testFri);

                // Pick a target that will take less than 2 seconds to get to
                Pose target = new Pose(new Vector3(0.1, 0.1, 0.1), new Quaternion(0, 0, 1, 0));

                // Create a pipeline and send a single Kinect message
                AgentLocator producerLocator = new AgentLocator();

                TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 10, 1);

                KukaLBRControlAgent manipAgent = new KukaLBRControlAgent(
                            "manipulationAgent",
                            timer,
                            armControl,
                            0.75,
                            1,
                            0.5,
                            false,
                            1);

                IAgentHost timerHost = producerLocator.Register(timer);
                manipHost = producerLocator.Register(manipAgent);
                IForwarder<LinearCartesianTouchControlMessage> armControlInjector = manipHost.GetControlPort<LinearCartesianTouchControlMessage>();

                MessageCollectorAgent<ManipulatorStatusMessage> collector = new MessageCollectorAgent<ManipulatorStatusMessage>("collector", manipAgent);

                long originatingTime = 10;

                double tolerance = 0.001;

                manipHost.Activate();
                LinearCartesianTouchControlMessage armMoveMessage = new LinearCartesianTouchControlMessage(
                    originatingTime,
                    target,
                    0.1,
                    0.1,
                    1);

                // Wait for FRI control to start
                while (testFri.GetRequestCount <= 1)
                {
                    Thread.Sleep(testFri.CycleTimeinMsec);
                }

                armControlInjector.Post(armMoveMessage);

                collectorHost = producerLocator.Register(collector);
                collectorHost.Activate();
                collector.Wait(500);

                long startTicks = Stopwatch.GetTimestamp();
                elapsedTicks = 0;

                // TimeOut in 5 Sec -- 
                timeOutTicks = Stopwatch.Frequency * 5;
                Pose current = new Pose();

                do
                {
                    if (null != collector.Results && collector.Results.Count > 0)
                    {
                        ManipulatorStatusMessage statusMsg = collector.Results.Dequeue();
                        current = statusMsg.State.TcpCartesianPosition;
                        unexpectedContact = statusMsg.State.TcpUnexpectedForceOccured;
                    }

                    elapsedTicks = Stopwatch.GetTimestamp() - startTicks;
                }
                while (
                    (Quaternion.Length(current.Orientation - target.Orientation) > tolerance ||
                    current.Position.DistanceFrom(target.Position) > tolerance) &&
                elapsedTicks <= timeOutTicks);
            }
            finally
            {
                if (collectorHost != null)
                {
                    collectorHost.Deactivate();
                }

                if (manipHost != null)
                {
                    manipHost.Deactivate();
                }
            }

            // if force from simulated FRI does not generate a stop, then target position will be reached
            // within time out, so pass iff we have timed out without reaching target position
            Assert.IsTrue(elapsedTicks > timeOutTicks, "Reached target position unexpectedly");
            Assert.IsTrue(unexpectedContact, "Contact was expected, but none was detected");
        }

        /// <summary>
        /// Used to test LBR Cartesian push
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [DeploymentItem("Microsoft.Robotics.Manipulation.ControllerStateMachine.dll")]
        [DeploymentItem("FRI_Remote.lib")]
        [DeploymentItem("Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4Controller.dll")]
        public void ManipulationKukaArmCartesianAgentTestPushForce()
        {
            IAgentHost manipHost = null;
            IAgentHost collectorHost = null;
            long timeOutTicks = 0;
            long elapsedTicks = 0;
            bool unexpectedContact = false;

            try
            {
                FriSimulator testFri = new FriSimulator();
                testFri.TcpForce = new Vector3(0, 0, 2);

                ArmControlKuka armControl = new ArmControlKuka(testFri);

                // Pick a target that will take less than 2 seconds to get to
                Pose target = new Pose(new Vector3(0.1, 0.1, 0.1), new Quaternion(0, 0, 1, 0));

                // Create a pipeline and send a single Kinect message
                AgentLocator producerLocator = new AgentLocator();

                TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 10, 1);

                KukaLBRControlAgent manipAgent = new KukaLBRControlAgent(
                            "manipulationAgent",
                            timer,
                            armControl,
                            0.75,
                            1,
                            0.5,
                            false,
                            1);

                IAgentHost timerHost = producerLocator.Register(timer);
                manipHost = producerLocator.Register(manipAgent);
                IForwarder<LinearCartesianPushControlMessage> armControlInjector = manipHost.GetControlPort<LinearCartesianPushControlMessage>();

                MessageCollectorAgent<ManipulatorStatusMessage> collector = new MessageCollectorAgent<ManipulatorStatusMessage>("collector", manipAgent);

                long originatingTime = 10;

                double tolerance = 0.001;

                manipHost.Activate();
                LinearCartesianPushControlMessage armMoveMessage = new LinearCartesianPushControlMessage(
                    originatingTime,
                    target,
                    0.1,
                    0.1,
                    new Vector3(0, 0, 1),
                    5,
                    1);

                // Wait for FRI control to start
                while (testFri.GetRequestCount <= 1)
                {
                    Thread.Sleep(testFri.CycleTimeinMsec);
                }

                armControlInjector.Post(armMoveMessage);

                collectorHost = producerLocator.Register(collector);
                collectorHost.Activate();
                collector.Wait(500);

                long startTicks = Stopwatch.GetTimestamp();
                elapsedTicks = 0;

                // TimeOut in 5 Sec -- 
                timeOutTicks = Stopwatch.Frequency * 5;
                Pose current = new Pose();

                do
                {
                    if (null != collector.Results && collector.Results.Count > 0)
                    {
                        ManipulatorStatusMessage statusMsg = collector.Results.Dequeue();
                        current = statusMsg.State.TcpCartesianPosition;
                        unexpectedContact = statusMsg.State.TcpUnexpectedForceOccured;
                    }

                    elapsedTicks = Stopwatch.GetTimestamp() - startTicks;
                }
                while (
                    (Quaternion.Length(current.Orientation - target.Orientation) > tolerance ||
                    current.Position.DistanceFrom(target.Position) > tolerance) &&
                elapsedTicks <= timeOutTicks);
            }
            finally
            {
                if (collectorHost != null)
                {
                    collectorHost.Deactivate();
                }

                if (manipHost != null)
                {
                    manipHost.Deactivate();
                }
            }

            // if force from simulated FRI does not generate a stop, then target position will be reached
            // which is expected result when force is in pushing direction (and below threshold)
            Assert.IsTrue(elapsedTicks < timeOutTicks, "Failed to reach target position");
            Assert.IsFalse(unexpectedContact, "Unexpected contact");
        }

        /// <summary>
        /// Used to test LBR Cartesian push with force orthogonal to push direction
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [DeploymentItem("Microsoft.Robotics.Manipulation.ControllerStateMachine.dll")]
        [DeploymentItem("FRI_Remote.lib")]
        [DeploymentItem("Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4Controller.dll")]
        public void ManipulationKukaArmCartesianAgentTestOrthogonalPushForce()
        {
            IAgentHost manipHost = null;
            IAgentHost collectorHost = null;
            long timeOutTicks = 0;
            long elapsedTicks = 0;
            bool unexpectedContact = false;

            try
            {
                FriSimulator testFri = new FriSimulator();
                testFri.TcpForce = new Vector3(0, 2, 0);

                ArmControlKuka armControl = new ArmControlKuka(testFri);

                // Pick a target that will take less than 2 seconds to get to
                Pose target = new Pose(new Vector3(0.1, 0.1, 0.1), new Quaternion(0, 0, 1, 0));

                // Create a pipeline and send a single Kinect message
                AgentLocator producerLocator = new AgentLocator();

                TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 10, 1);

                KukaLBRControlAgent manipAgent = new KukaLBRControlAgent(
                            "manipulationAgent",
                            timer,
                            armControl,
                            0.75,
                            1,
                            0.5,
                            false,
                            1);

                IAgentHost timerHost = producerLocator.Register(timer);
                manipHost = producerLocator.Register(manipAgent);
                IForwarder<LinearCartesianPushControlMessage> armControlInjector = manipHost.GetControlPort<LinearCartesianPushControlMessage>();

                MessageCollectorAgent<ManipulatorStatusMessage> collector = new MessageCollectorAgent<ManipulatorStatusMessage>("collector", manipAgent);

                long originatingTime = 10;

                double tolerance = 0.001;

                manipHost.Activate();
                LinearCartesianPushControlMessage armMoveMessage = new LinearCartesianPushControlMessage(
                    originatingTime,
                    target,
                    0.1,
                    0.1,
                    new Vector3(0, 0, 1),
                    5,
                    1);

                // Wait for FRI control to start
                while (testFri.GetRequestCount <= 1)
                {
                    Thread.Sleep(testFri.CycleTimeinMsec);
                }

                armControlInjector.Post(armMoveMessage);

                collectorHost = producerLocator.Register(collector);
                collectorHost.Activate();
                collector.Wait(500);

                long startTicks = Stopwatch.GetTimestamp();
                elapsedTicks = 0;

                // TimeOut in 5 Sec -- 
                timeOutTicks = Stopwatch.Frequency * 5;
                Pose current = new Pose();

                do
                {
                    if (null != collector.Results && collector.Results.Count > 0)
                    {
                        ManipulatorStatusMessage statusMsg = collector.Results.Dequeue();
                        current = statusMsg.State.TcpCartesianPosition;
                        unexpectedContact = statusMsg.State.TcpUnexpectedForceOccured;
                    }

                    elapsedTicks = Stopwatch.GetTimestamp() - startTicks;
                }
                while (
                    (Quaternion.Length(current.Orientation - target.Orientation) > tolerance ||
                    current.Position.DistanceFrom(target.Position) > tolerance) &&
                elapsedTicks <= timeOutTicks);
            }
            finally
            {
                if (collectorHost != null)
                {
                    collectorHost.Deactivate();
                }

                if (manipHost != null)
                {
                    manipHost.Deactivate();
                }
            }

            // if force from simulated FRI does not generate a stop, then target position will be reached
            // within time out, so pass iff we have timed out without reaching target position
            Assert.IsTrue(elapsedTicks > timeOutTicks, "Reached target position unexpectedly");
            Assert.IsTrue(unexpectedContact, "Contact was expected, but none was detected");
        }

        /// <summary>
        /// Build a manipulation agent pipeline with the manipulation agents and a test FRI.
        /// Send a joint move message down the pipeline and wait till
        /// status messages show the move is complete
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationKukaArmJointMoveAgentTest()
        {
            FriSimulator testFri = new FriSimulator();
            ArmControlKuka armControl = new ArmControlKuka(testFri);

            // Create a pipeline and send a single Kinect message
            AgentLocator producerLocator = new AgentLocator();

            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 1, 1);

            KukaLBRControlAgent manipAgent = new KukaLBRControlAgent(
                        "manipulationAgent",
                        timer,
                        armControl,
                        0.25,
                        0.3,
                        Math.PI / 4.0,
                        false,
                        1);

            IAgentHost timerHost = producerLocator.Register(timer);
            IAgentHost manipHost = producerLocator.Register(manipAgent);
            IForwarder<JointGoalPositionControlMessage> armControlInjector = manipHost.GetControlPort<JointGoalPositionControlMessage>();

            MessageCollectorAgent<ManipulatorStatusMessage> collector = new MessageCollectorAgent<ManipulatorStatusMessage>("collector", manipAgent);

            long originatingTime = AgentMessage.GetCurrentTime();
            double[] target = new double[KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS];
            Random rand = new Random(10);
            for (int i = 0; i < target.Length; ++i)
            {
                target[i] = rand.NextDouble() * MathConstants.TwoPI - Math.PI;
            }

            double tolerance = 0.001;

            manipHost.Activate();
            JointGoalPositionControlMessage armMoveMessage = new JointGoalPositionControlMessage(originatingTime, target, tolerance);

            // Wait for FRI control to start
            while (testFri.GetRequestCount <= 1)
            {
                Thread.Sleep(testFri.CycleTimeinMsec);
            }

            armControlInjector.Post(armMoveMessage);

            IAgentHost collectorHost = producerLocator.Register(collector);

            try
            {
                collectorHost.Activate();
                collector.Wait(500);

                long startTicks = Stopwatch.GetTimestamp();
                long elapsedTicks = 0;

                // TimeOut in 1 Sec
                long timeOutTicks = Stopwatch.Frequency * 1;
                double maxAbsDiff;
                maxAbsDiff = 2 * tolerance;

                double[] currentVelocity;
                double[] currentPosition;
                double maxAbsVelocity = 0;

                do
                {
                    if (null != collector.Results && collector.Results.Count > 0)
                    {
                        ManipulatorStatusMessage statusMsg = collector.Results.Dequeue();
                        currentPosition = statusMsg.State.JointPositions;
                        currentVelocity = statusMsg.State.JointVelocities;

                        maxAbsDiff = Math.Abs(currentPosition[0] - target[0]);
                        for (int i = 1; i < currentPosition.Length; ++i)
                        {
                            maxAbsDiff = Math.Max(Math.Abs(currentPosition[i] - target[i]), maxAbsDiff);
                            maxAbsVelocity = Math.Max(Math.Abs(currentVelocity[i]), maxAbsVelocity);
                        }
                    }

                    elapsedTicks = Stopwatch.GetTimestamp() - startTicks;
                }
                while (maxAbsDiff > tolerance && elapsedTicks <= timeOutTicks);

                // simply check that we are getting some sort of valid velocity values during move
                Assert.AreNotEqual(0, maxAbsVelocity);

                Assert.IsTrue(elapsedTicks <= timeOutTicks, "ManipulationAgent test timed out");
            }
            finally
            {
                collectorHost.Deactivate();
                manipHost.Deactivate();
            }
        }

        /// <summary>
        /// Build a manipulation agent pipeline with the manipulation agents.
        /// Send a joint move message down the pipeline and test the joint
        /// position controller with the actual robot arm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
#if !KUKA_LBR_PRESENT
        [Ignore]
#endif
        public void HW_ManipulationKukaArmJointMoveAgentTest()
        {
            ArmControlKuka armControl = new ArmControlKuka();

            // Create a pipeline and send a single Kinect message
            AgentLocator producerLocator = new AgentLocator();

            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 10, 1);

            KukaLBRControlAgent armControlAgent = new KukaLBRControlAgent(
                        "manipulationAgent",
                        timer,
                        armControl,
                        0.1, // maximum ee linear velocity m/s
                        0.1, // maximum ee angular velocity rad/s
                        0.1, // maximum joint angular velocity rad/s
                        false,
                        1);

            IAgentHost timerHost = producerLocator.Register(timer);
            IAgentHost manipHost = producerLocator.Register(armControlAgent);
            IForwarder<JointGoalPositionControlMessage> armControlInjector = manipHost.GetControlPort<JointGoalPositionControlMessage>();

            MessageCollectorAgent<ManipulatorStatusMessage> collector = new MessageCollectorAgent<ManipulatorStatusMessage>("collector", armControlAgent);

            long originatingTime = AgentMessage.GetCurrentTime();
            double[] target = this.GetJointTestPosition();

            double tolerance = 0.01;

            manipHost.Activate();
            JointGoalPositionControlMessage armMoveMessage = new JointGoalPositionControlMessage(originatingTime, target, tolerance);

            armControlInjector.Post(armMoveMessage);

            IAgentHost collectorHost = producerLocator.Register(collector);

            try
            {
                collectorHost.Activate();
                collector.Wait(500);

                long startTicks = Stopwatch.GetTimestamp();
                long elapsedTicks = 0;

                // TimeOut in 60 Sec since we could be testing in T1 mode, which is slow
                long timeOutTicks = Stopwatch.Frequency * 60;
                double maxAbsDiff;
                maxAbsDiff = 2 * tolerance;

                double[] current;
                do
                {
                    if (null != collector.Results && collector.Results.Count > 0)
                    {
                        ManipulatorStatusMessage statusMsg = collector.Results.Dequeue();
                        current = statusMsg.State.JointPositions;

                        maxAbsDiff = Math.Abs(current[0] - target[0]);
                        for (int i = 1; i < current.Length; ++i)
                        {
                            maxAbsDiff = Math.Max(Math.Abs(current[i] - target[i]), maxAbsDiff);
                        }
                    }

                    elapsedTicks = Stopwatch.GetTimestamp() - startTicks;
                }
                while (maxAbsDiff > tolerance && elapsedTicks <= timeOutTicks);

                Assert.IsTrue(elapsedTicks <= timeOutTicks, "ManipulationAgent test timed out");
            }
            finally
            {
                collectorHost.Deactivate();
                manipHost.Deactivate();
            }
        }

        /// <summary>
        /// Build a manipulation agent pipeline with the manipulation agents and a RDS Sim FRI.
        /// Send a joint move message to the pipeline and wait for
        /// status messages to show the move is complete
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [Ignore]
        #region DeployItems
        [DeploymentItem("Microsoft.Robotics.Hardware.KukaAPI.dll")]
        [DeploymentItem("Microsoft.Robotics.Manipulation.ControllerStateMachine.dll")]
        [DeploymentItem("Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4Controller.dll")]
        [DeploymentItem("Microsoft.Robotics.Manipulation.ControllerStateMachine.dll")]
        [DeploymentItem("Microsoft.Robotics.Manipulation.ControllerStateMachine.dll")]
        [DeploymentItem("Microsoft.Ccr.Adapters.IO.dll")]
        [DeploymentItem("Microsoft.Ccr.Adapters.WinForms.dll")]
        [DeploymentItem("Microsoft.Ccr.Adapters.Wpf.dll")]
        [DeploymentItem("Microsoft.Ccr.Core.dll")]
        [DeploymentItem("Microsoft.Dss.Base.dll")]
        [DeploymentItem("Microsoft.Dss.Diagnostics.dll")]
        [DeploymentItem("Microsoft.Dss.Environment.dll")]
        [DeploymentItem("Microsoft.Dss.Runtime.dll")]
        [DeploymentItem("Microsoft.Dss.Runtime.Proxy.dll")]
        [DeploymentItem("Microsoft.Dss.Runtime.Transform.dll")]
        [DeploymentItem("Microsoft.Dss.Tools.dll")]
        [DeploymentItem("Microsoft.Dss.Tools.VisualStudio.Wizards.dll")]
        [DeploymentItem("RoboticsCommon.proxy.dll")]
        [DeploymentItem("SimulatedLBR3Arm.Y2006.M07.proxy.dll")]
        [DeploymentItem("SimulationCommon.proxy.dll")]
        [DeploymentItem("SimulationEngine.proxy.dll")]
        [DeploymentItem("en\\Microsoft.Ccr.Adapters.IO.xml", "en\\")]
        [DeploymentItem("en\\Microsoft.Ccr.Adapters.WinForms.xml", "en\\")]
        [DeploymentItem("en\\Microsoft.Ccr.Adapters.Wpf.xml", "en\\")]
        [DeploymentItem("en\\Microsoft.Ccr.Core.xml", "en\\")]
        [DeploymentItem("en\\Microsoft.Dss.Base.xml", "en\\")]
        [DeploymentItem("en\\Microsoft.Dss.Diagnostics.xml", "en\\")]
        [DeploymentItem("en\\Microsoft.Dss.Environment.xml", "en\\")]
        [DeploymentItem("en\\Microsoft.Dss.Runtime.proxy.xml", "en\\")]
        [DeploymentItem("en\\Microsoft.Dss.Runtime.xml", "en\\")]
        [DeploymentItem("en\\Microsoft.Dss.Tools.xml", "en\\")]
        [DeploymentItem("en\\RoboticsCommon.proxy.xml", "en\\")]
        [DeploymentItem("en\\SimulationCommon.proxy.xml", "en\\")]
        #endregion
        public void ManipulationKukaArmJointMoveAgentRdsSimTest()
        {
            LbrRdsSimManagedFri simFri = new LbrRdsSimManagedFri();

            try
            {
                simFri.Initialize();
            }
            catch (ApplicationException)
            {
                Assert.Fail("Failed to find RDS LBR sim");
            }

            ArmControlKuka armControl = new ArmControlKuka(simFri);

            // Create a pipeline and send a single Kinect message
            AgentLocator producerLocator = new AgentLocator();

            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 1, 1);

            KukaLBRControlAgent manipAgent = new KukaLBRControlAgent(
                        "manipulationAgent",
                        timer,
                        armControl,
                        0.25,
                        0.5,
                        0.5,
                        false,
                        1);

            IAgentHost timerHost = producerLocator.Register(timer);
            IAgentHost manipHost = producerLocator.Register(manipAgent);
            IForwarder<JointGoalPositionControlMessage> armControlInjector = manipHost.GetControlPort<JointGoalPositionControlMessage>();

            MessageCollectorAgent<ManipulatorStatusMessage> collector = new MessageCollectorAgent<ManipulatorStatusMessage>("collector", manipAgent);

            long originatingTime = AgentMessage.GetCurrentTime();
            double[] target = new double[KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS];
            Random rand = new Random(10);
            for (int i = 0; i < target.Length; ++i)
            {
                // scaling targets so sim arm doesn't get into impossible configs
                target[i] = (rand.NextDouble() * MathConstants.TwoPI - Math.PI) / 8.0;
            }

            double tolerance = 0.01;

            manipHost.Activate();
            timerHost.Activate();

            IAgentHost collectorHost = producerLocator.Register(collector);
            collectorHost.Activate();
            collector.Wait(500);

            JointGoalPositionControlMessage armMoveMessage = new JointGoalPositionControlMessage(originatingTime, target, tolerance);

            armControlInjector.Post(armMoveMessage);

            try
            {
                long startTicks = Stopwatch.GetTimestamp();
                long elapsedTicks = 0;

                // TimeOut in 60 Sec
                long timeOutTicks = Stopwatch.Frequency * 600;
                double maxAbsDiff;
                maxAbsDiff = 2 * tolerance;

                double[] current;
                do
                {
                    if (null != collector.Results && collector.Results.Count > 0)
                    {
                        ManipulatorStatusMessage statusMsg = collector.Results.Dequeue();
                        current = statusMsg.State.JointPositions;

                        maxAbsDiff = Math.Abs(current[0] - target[0]);
                        for (int i = 1; i < current.Length; ++i)
                        {
                            maxAbsDiff = Math.Max(Math.Abs(current[i] - target[i]), maxAbsDiff);
                        }
                    }

                    elapsedTicks = Stopwatch.GetTimestamp() - startTicks;
                }
                while (maxAbsDiff > tolerance && elapsedTicks <= timeOutTicks);

                Assert.IsTrue(elapsedTicks <= timeOutTicks, "ManipulationAgent test timed out");
            }
            finally
            {
                timerHost.Deactivate();
                collectorHost.Deactivate();
                manipHost.Deactivate();
            }
        }

        /// <summary>
        /// Test a cartesian move using KukaLBRSimpleCartesianMoveAgent
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        #region DeployItems
        [DeploymentItem("Microsoft.Robotics.Hardware.KukaAPI.dll")]
        [DeploymentItem("Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4Controller.dll")]
        [DeploymentItem("Microsoft.Robotics.Manipulation.ControllerStateMachine.dll")]
        [DeploymentItem(@"ShoArray.dll")]
        [DeploymentItem(@"MatrixInterf.dll")]
        [DeploymentItem(@"ArrayT.dll")]
        [DeploymentItem(@"libiomp5md.dll")]
        [DeploymentItem(@"mkl.dll")]
        #endregion
        public void ManipulationKukaLBRSimpleCartesianMoveAgentTest()
        {
            FriSimulator testFri = new FriSimulator();
            ArmControlKuka armControl = new ArmControlKuka(testFri);

            AgentLocator producerLocator = new AgentLocator(true);
            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 1, 1);

            KukaLBRControlAgent manipAgent = new KukaLBRControlAgent(
                        "manipulationAgent",
                        timer,
                        armControl,
                        0.25,
                        0.3,
                        1000, // this velocity limit is insanely large, only use it in simulation
                        false,
                        1);

            ArmCubicSplineExecutionAgent splineAgent = new ArmCubicSplineExecutionAgent(
                name: "CubicSplineAgent",
                producer: manipAgent,
                armJointControlAgentName: manipAgent.Name,
                jointErrorLimit: 0);

            KukaLBRSimpleCartesianMoveAgent moveAgent = new KukaLBRSimpleCartesianMoveAgent(
                name: "KukaLBRSimpleCartesianMoveAgent",
                producer: manipAgent,
                trajectoryControlAgent: splineAgent,
                jointTolerance: 0.01,
                minimumJointHeight: 0.08,
                maximumJointVel: Math.PI / 4,
                cutOffPercentageForIKFiltering: 0.8,
                minimumTrajectoryTime: 2.0,
                guaranteedClearance: 0.01,
                minimumElbowHeight: 0.2,
                trajectoryQualityThreshold: 10.0,
                jointWeightFactors: new double[] { 1, 1.5, 1, 1, 1.5, 1, 0.5 },
                elbowClearanceCheckList: new bool[] { false, false, true, false, false, false, false });

            IAgentHost timerHost = producerLocator.Register(timer);
            IAgentHost manipHost = producerLocator.Register(manipAgent);
            IAgentHost splineHost = producerLocator.Register(splineAgent);
            IAgentHost moveHost = producerLocator.Register(moveAgent);
            
            // We wait for a big number of messages to come, since the producer locator was initialized in offline mode
            // it is guaranteed that no messages can be dropped.  The number 10 is bigger than the size of a message
            // queue.  Thus, the first message to the moveAgent should be received during the warm up of the collector.
            MessageCollectorAgent<ManipulatorStatusMessage> collector = new MessageCollectorAgent<ManipulatorStatusMessage>("collector", manipAgent, 10);
            IAgentHost collectorHost = producerLocator.Register(collector);

            splineHost.Activate();
            moveHost.Activate();
            collectorHost.Activate();

            IForwarder<CartesianGoalPositionControlMessage> motionRequestInjector = moveHost.GetControlPort<CartesianGoalPositionControlMessage>();

            Pose[] targets = new Pose[5];
            targets[0] = new Pose(new Vector3(-0.553, 0.000, 0.413), new Quaternion(0.282, -0.681, -0.259, 0.624));
            targets[1] = new Pose(new Vector3(-0.120, -0.880, 0.079), new Quaternion(0.660, -0.328, 0.216, 0.640));
            targets[2] = new Pose(new Vector3(-0.464, -0.488, 0.216), new Quaternion(0.716, -0.698, 0.000, 0.000));
            targets[3] = new Pose(new Vector3(0.278, -0.499, 0.341), new Quaternion(0.990, -0.138, 0.000, 0.000));
            targets[4] = new Pose(new Vector3(-0.353, -0.465, 0.036), new Quaternion(0.556, -0.485, 0.046, 0.674));

            double translationalTolerance = 0.01;
            double rotationalTolerance = 0.05;

            try
            {
                // wait for the manipulation messages to arrive
                bool messageCollected = collector.Wait(200);
                Assert.IsTrue(messageCollected, "Status Message was not collected before time out");

                for (int i = 0; i < targets.Length; ++i)
                {
                    Pose target = targets[i];

                    // send the command to move the arm
                    CartesianGoalPositionControlMessage msg = new CartesianGoalPositionControlMessage(AgentMessage.GetCurrentTime(), target, translationalTolerance, rotationalTolerance);
                    motionRequestInjector.Post(msg);

                    long elapsedTicks = 0;
                    long timeOutTicks = Stopwatch.Frequency * 5;

                    this.MonitorCartesianMoveProcess(collector, target, this.ComputeEEPoseFromJoints, timeOutTicks, out elapsedTicks, translationalTolerance, rotationalTolerance);
                    
                    Assert.IsTrue(elapsedTicks <= timeOutTicks, "ManipulationAgent test timed out");
                }
            }
            finally
            {
                collectorHost.Deactivate();
                splineHost.Deactivate();
                moveHost.Deactivate();
            }
        }

        /// <summary>
        /// Used to test spline based moves of the LBR
        /// </summary>
        /// <param name="cs"> The spline to execute. Should consist of two points.</param>
        /// <param name="completionMargin">Amount of time in seconds after expected motion completion to timeout. </param>
        /// <param name="tolerance">Tolerance for considering two joint positions equal.</param>
        /// <param name="positionErrorLimit">Max distance between actual and commanded positions.</param>
        public void ManipulationKukaSplineAgentTest(CubicSpline cs, double completionMargin, double tolerance, double positionErrorLimit = 0)
        {
            Assert.AreEqual(7, cs.TargetPositions.GetLength(0), "Check number of joint position targets given");
            FriSimulator testFri = new FriSimulator();
            ArmControlKuka armControl = new ArmControlKuka(testFri);

            // Create a pipeline using a simulated FRI
            AgentLocator producerLocator = new AgentLocator();

            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 10, 1);

            KukaLBRControlAgent manipAgent = new KukaLBRControlAgent(
                        "manipulationAgent",
                        timer,
                        armControl,
                        0.75,
                        1,
                        0.5, 
                        false,
                        1);

            ArmCubicSplineExecutionAgent splineAgent = new ArmCubicSplineExecutionAgent(
                "splineAgent",
                manipAgent,
                "manipulationAgent",
                positionErrorLimit);

            IAgentHost timerHost = producerLocator.Register(timer);
            IAgentHost manipHost = producerLocator.Register(manipAgent);
            IAgentHost splineHost = producerLocator.Register(splineAgent);

            IForwarder<StartTrajectoryMessage> splineInjector = splineHost.GetControlPort<StartTrajectoryMessage>();

            MessageCollectorAgent<ManipulatorStatusMessage> collector = new MessageCollectorAgent<ManipulatorStatusMessage>("collector", manipAgent);
            IAgentHost collectorHost = producerLocator.Register(collector);

            long originatingTime = 10;

            try
            {
                manipHost.Activate();
                splineHost.Activate();

                // Wait for FRI control to start
                while (testFri.GetRequestCount <= 1)
                {
                    Thread.Sleep(testFri.CycleTimeinMsec);
                }

                StartTrajectoryMessage startSplineMessage = new StartTrajectoryMessage(
                    originatingTime,
                    cs.NumDOF,
                    cs.NumPoints,
                    cs.Times,
                    cs.TargetPositions,
                    cs.TargetVelocities,
                    TrajectoryInterpolationMethods.CubicSpline,
                    true);

                splineInjector.Post(startSplineMessage);

                collectorHost.Activate();
                collector.Wait(500);

                double[] currentPosition;
                long startTicks = Stopwatch.GetTimestamp();
                long elapsedTicks = 0;

                // TimeOut after completion time + margin
                long timeOutTicks = (long)(Stopwatch.Frequency * (cs.Times[1] + completionMargin));

                double maxPosError = 2 * tolerance;
                double currentError = 0;
                do
                {
                    if (null != collector.Results && collector.Results.Count > 0)
                    {
                        ManipulatorStatusMessage statusMsg = collector.Results.Dequeue();
                        currentPosition = statusMsg.State.JointPositions;

                        elapsedTicks = Stopwatch.GetTimestamp() - startTicks;

                        maxPosError = 0;
                        for (int i = 0; i < currentPosition.Length; ++i)
                        {
                            Assert.IsFalse(double.IsNaN(currentPosition[i]) || double.IsInfinity(currentPosition[i]), "Check for valid valid");
                            currentError = Math.Abs(currentPosition[i] - cs.TargetPositions[i, 1]);
                            if (currentError > maxPosError)
                            {
                                maxPosError = currentError;
                            }
                        }
                    }
                }
                while (maxPosError > tolerance && elapsedTicks <= timeOutTicks);

                Assert.IsTrue(elapsedTicks <= timeOutTicks, "ManipulationAgent test timed out");
            }
            finally
            {
                collectorHost.Deactivate();
                manipHost.Deactivate();
                splineHost.Deactivate();
            }
        }

        /// <summary>
        /// Compute pose of end effector based on joint positions
        /// </summary>
        /// <param name="status">Arm Status</param>
        /// <returns>End effector pose</returns>
        private Pose ComputeEEPoseFromJoints(ManipulatorStatus status)
        {
            KinematicChain kc = KukaLBR4KinematicConfigurations.CreateKuka7DOFKinematicChain();
            return new Pose(kc.GetEndEffectorTransform(DoubleArray.From(status.JointPositions).Transpose()));
        }

        /// <summary>
        /// Extract pose from status
        /// </summary>
        /// <param name="status">Status of the arm</param>
        /// <returns>The pose of the end effector</returns>
        private Pose ExtractPoseFromStatus(ManipulatorStatus status)
        {
            return status.TcpCartesianPosition;
        }

        /// <summary>
        /// Monitor a Cartesian move process
        /// </summary>
        /// <param name="collector">The collector to get messages</param>
        /// <param name="target">Target pose</param>
        /// <param name="getPoseFunc">Method to get the pose of the end effector</param>
        /// <param name="timeOutTicks">Number of ticks to determine time out</param>
        /// <param name="elapsedTicks">Elapsed ticks</param>
        /// <param name="posTolerance">Positional tolerance</param>
        /// <param name="orientTolerance">Rotational tolerance</param>
        private void MonitorCartesianMoveProcess(MessageCollectorAgent<ManipulatorStatusMessage> collector, Pose target, GetCurrentPoseFromStatus getPoseFunc, double timeOutTicks, out long elapsedTicks, double posTolerance = 0.005, double orientTolerance = 0.01)
        {
            // monitor the status
            long startTicks = Stopwatch.GetTimestamp();
            elapsedTicks = 0;

            double orientationDist = 0.0;
            double euclidianDist = 0.0;
            ManipulatorStatus currentStatus = null;
            Pose current = new Pose();

            do
            {
                if (null != collector.Results && collector.Results.Count > 0)
                {
                    ManipulatorStatusMessage statusMsg = collector.Results.Dequeue();
                    currentStatus = statusMsg.State;

                elapsedTicks = Stopwatch.GetTimestamp() - startTicks;

                current = getPoseFunc(currentStatus);
                orientationDist = Quaternion.Length(current.Orientation - target.Orientation);
                euclidianDist = current.Position.DistanceFrom(target.Position);
                Assert.IsFalse(double.IsNaN(orientationDist), "Numerical error in Quaternion update");
                Assert.IsFalse(double.IsNaN(euclidianDist), "Numerical error in Position update");
            }
            }
            while (false == this.IsArmMoveComplete(current, target, orientTolerance, posTolerance) && elapsedTicks <= timeOutTicks);
        }

        /// <summary>
        /// get a joint position for test
        /// </summary>
        /// <returns>a valid joint position configuration</returns>
        private double[] GetJointTestPosition()
        {
            double[] joints = { 0, 0, 0, 0, 0, 0, 0 };
            return joints;
        }

        /// <summary>
        /// Decides if an arm move action is complete
        /// </summary>
        /// <param name="current"> Current pose. </param>
        /// <param name="target"> Target pose. </param>
        /// <param name="armOrientationTolerance"> Orientation tolerance. </param>
        /// <param name="armPositionTolerance"> Position tolerance. </param>
        /// <returns>True if the move is complete</returns>
        private bool IsArmMoveComplete(Pose current, Pose target, double armOrientationTolerance, double armPositionTolerance)
        {
            double quatDiff = Quaternion.Length(current.Orientation - target.Orientation);
            double posDiff = current.Position.DistanceFrom(target.Position);
            bool ready = quatDiff < armOrientationTolerance &&
                posDiff < armPositionTolerance;

            if (quatDiff > armOrientationTolerance && Math.Abs(target.Orientation.W) < 0.001)
            {
                // Corner case check for equivalent rotations for ~180' rotations  (w small)
                if (Math.Abs(Math.Abs(target.Orientation.X) - Math.Abs(current.Orientation.X)) < armOrientationTolerance &&
                    Math.Abs(Math.Abs(target.Orientation.Y) - Math.Abs(current.Orientation.Y)) < armOrientationTolerance &&
                    Math.Abs(Math.Abs(target.Orientation.Z) - Math.Abs(current.Orientation.Z)) < armOrientationTolerance &&
                    Math.Abs(Math.Abs(target.Orientation.W) - Math.Abs(current.Orientation.W)) < armOrientationTolerance)
                {
                    ready = posDiff < armPositionTolerance;
                }
            }

            return ready;
        }
   }
}
